#include "nxColor.h"
#include "cmvision/conversions.h"

#include <cxcore.h>
#include <iostream>
#include <cstdio>

using namespace std;
 
/********************************************
 * CONSTRUCTOR
 ********************************************/

nxColor::nxColor(char* name, CvScalar mean[], CvScalar stdDev[])
{
	sprintf(nameC,"%s",name);
	meanValue_RGB = mean[0];
	meanValue_HLS = mean[1];
	meanValue_HSV = mean[2];
	meanValue_YUV = mean[3];
	
	stdDev_RGB = stdDev[0];
	stdDev_HLS = stdDev[1];
	stdDev_HSV = stdDev[2];
	stdDev_YUV = stdDev[3];

	resetUmbrals();
	
}
nxColor::nxColor(char* name,IplImage* cvImg,int colorCode)
{
	
	//this->nameC = name;
	sprintf(nameC,"%s",name);
	IplImage* imgHSV;
	IplImage* imgHLS;
	IplImage* imgRGB;
	IplImage* imgYUV;
	
	
	//Images to be used for mean and stdDev extraction
	imgHSV = cvCreateImage(cvGetSize(cvImg),IPL_DEPTH_8U,3);
	imgHLS = cvCreateImage(cvGetSize(cvImg),IPL_DEPTH_8U,3);
	imgRGB = cvCreateImage(cvGetSize(cvImg),IPL_DEPTH_8U,3);
	imgYUV = cvCreateImage(cvGetSize(cvImg),IPL_DEPTH_8U,4);

	switch(colorCode)
	{
		case BGR_CODE:
			
			cvCvtColor(cvImg,imgHSV,CV_BGR2HSV);
			cvCvtColor(cvImg,imgHLS,CV_BGR2HLS);
			cvCvtColor(cvImg,imgRGB,CV_BGR2RGB);
			
		break;

		case RGB_CODE:
			
			cvCvtColor(cvImg,imgHSV,CV_RGB2HSV);
			cvCvtColor(cvImg,imgHLS,CV_RGB2HLS);

			cvCopy(cvImg,imgRGB);

			//Convert to YUV format in UYVY

			//Size of image ROI (REgion of interest, in case zoom is being used)
			CvSize size;
			int step;
			unsigned char *rawData;

			cvGetRawData( cvImg, &rawData,&step,&size);

			//unsigned char yuvData[size.height*size.width*(channels+1)];

			printf("Conversion a YUV\n");
			//TODO: No esta funcionando correctamente
			//rgb2uyvy (rawData, (unsigned char*)imgYUV->imageData, size.height*size.width);

			//imgYUV->imageData = yuvData;

		break;

		case HSV_CODE:


			cvCvtColor(cvImg,imgRGB,CV_HSV2RGB);
			nxColor(name,imgRGB,RGB_CODE);
					
		break;

		case HLS_CODE:
		
			cvCvtColor(cvImg,imgRGB,CV_HLS2RGB);
			nxColor(name,imgRGB,RGB_CODE);			
					
		break;

		case YUV_CODE:

			//TODO: NO ESTA LISTA ESTA PARTE!!!
			cvCvtColor(cvImg,imgRGB,CV_HLS2RGB);
			//rgb2uyvy (unsigned char *src, unsigned char *dest, unsigned long long int NumPixels);

			nxColor(name,imgRGB,RGB_CODE);

		break;

	}
	
	cvAvgSdv( imgRGB, &meanValue_RGB, &stdDev_RGB,NULL);
	cvAvgSdv( imgHSV, &meanValue_HSV, &stdDev_HSV,NULL);
	cvAvgSdv( imgHLS, &meanValue_HLS, &stdDev_HLS,NULL);
	cvAvgSdv( imgYUV, &meanValue_YUV, &stdDev_YUV,NULL);

	resetUmbrals();
	
	cvReleaseImage(&imgHSV);
	cvReleaseImage(&imgHLS);
	cvReleaseImage(&imgRGB);	
	cvReleaseImage(&imgYUV);
	
}
 /********************************************
 * DESTRUCTOR
 ********************************************/
 
nxColor::~nxColor()
{
	
}
/********************************************
 * GETMEANVALUE
 ********************************************/
 
CvScalar nxColor::getMeanValue(int colorCode)
{
	switch(colorCode)
	{
		case RGB_CODE:
			return meanValue_RGB;
		break;
		case HSV_CODE:
			return meanValue_HSV;
		break;
		case HLS_CODE:
			return meanValue_HLS;
		break;
		case YUV_CODE:
			return meanValue_YUV;
		break;
		
		default:
			return meanValue_RGB;
	
	}
	
}

int nxColor::getMeanValueInt(int colorCode)
{
	switch(colorCode)
	{
		case RGB_CODE:
			return scalar2int(meanValue_RGB);
		break;
		case HSV_CODE:
			return scalar2int(meanValue_HSV);
		break;
		case HLS_CODE:
			return scalar2int(meanValue_HLS);
		break;
		case YUV_CODE:
			return scalar2int(meanValue_YUV);
		break;

		
		default:
			return scalar2int(meanValue_RGB);
	
	}
}
int nxColor::scalar2int(CvScalar color)
{
	
	return int(color.val[0])   << 16 |
                      int(color.val[1]) <<  8 |
                      int(color.val[2]);
}
/********************************************
 * GETSTDDEV
 ********************************************/
 
CvScalar nxColor::getStdDev(int colorCode)
{
	switch(colorCode)
	{
		case RGB_CODE:
			return stdDev_RGB;
		break;
		case HSV_CODE:
			return stdDev_HSV;
		break;
		case HLS_CODE:
			return stdDev_HLS;
		break;
		case YUV_CODE:
			return stdDev_YUV;
		break;
		
		default:
			return stdDev_RGB;
	
	}
}		


CvScalar nxColor::getHighUmbral(int colorCode)
{
	switch(colorCode){
		case RGB_CODE:
			return hiUmbral_RGB;
		break;
		
		case HLS_CODE:
			return hiUmbral_HLS;
		break;
		
		case HSV_CODE:
			return hiUmbral_HSV;
		break;

		case YUV_CODE:
			return hiUmbral_YUV;
		break;
	}
}

CvScalar nxColor::getLowUmbral(int colorCode)
{
	switch(colorCode){
		case RGB_CODE:
			return lowUmbral_RGB;
		break;
		
		case HLS_CODE:
			return lowUmbral_HLS;
		break;
		
		case HSV_CODE:
			return lowUmbral_HSV;
		break;

		case YUV_CODE:
			return lowUmbral_YUV;
		break;
	}
}



void nxColor::setHighUmbral(CvScalar hiUmbral, int colorCode)
{
	switch(colorCode){
		case RGB_CODE:
			hiUmbral_RGB = hiUmbral;
		break;
		
		case HLS_CODE:
			hiUmbral_HLS = hiUmbral;
		break;
		
		case HSV_CODE:
			hiUmbral_HSV = hiUmbral;
		break;

		case YUV_CODE:
			hiUmbral_YUV = hiUmbral;
		break;

	}
}

void nxColor::setLowUmbral(CvScalar lowUmbral, int colorCode)
{
	switch(colorCode){
		case RGB_CODE:
			lowUmbral_RGB = lowUmbral;
		break;
		
		case HLS_CODE:
			lowUmbral_HLS = lowUmbral;
		break;
		
		case HSV_CODE:
			lowUmbral_HSV = lowUmbral;
		break;

		case YUV_CODE:
			lowUmbral_YUV = lowUmbral;
		break;

	}
}

void nxColor::resetUmbrals()
{
	lowUmbral_RGB = cvScalar(meanValue_RGB.val[0] - 4 * stdDev_RGB.val[0],meanValue_RGB.val[1] - 4 * stdDev_RGB.val[1],meanValue_RGB.val[2] - 4 * stdDev_RGB.val[2]);
	lowUmbral_HLS = cvScalar(meanValue_HLS.val[0] - 4 * stdDev_HLS.val[0],meanValue_HLS.val[1] - 4 * stdDev_HLS.val[1],meanValue_HLS.val[2] - 4 * stdDev_HLS.val[2]);
	lowUmbral_HSV = cvScalar(meanValue_HSV.val[0] - 4 * stdDev_HSV.val[0],meanValue_HSV.val[1] - 4 * stdDev_HSV.val[1],meanValue_HSV.val[2] - 4 * stdDev_HSV.val[2]);
	lowUmbral_YUV = cvScalar(meanValue_YUV.val[0] - 4 * stdDev_YUV.val[0],meanValue_YUV.val[1] - 4 * stdDev_YUV.val[1],meanValue_YUV.val[2] - 4 * stdDev_YUV.val[2]);

	hiUmbral_RGB = cvScalar(meanValue_RGB.val[0] + 4 * stdDev_RGB.val[0],meanValue_RGB.val[1] + 4 * stdDev_RGB.val[1],meanValue_RGB.val[2] + 4 * stdDev_RGB.val[2]);
	hiUmbral_HLS = cvScalar(meanValue_HLS.val[0] + 4 * stdDev_HLS.val[0],meanValue_HLS.val[1] + 4 * stdDev_HLS.val[1],meanValue_HLS.val[2] + 4 * stdDev_HLS.val[2]);
	hiUmbral_HSV = cvScalar(meanValue_HSV.val[0] + 4 * stdDev_HSV.val[0],meanValue_HSV.val[1] + 4 * stdDev_HSV.val[1],meanValue_HSV.val[2] + 4 * stdDev_HSV.val[2]);
	hiUmbral_YUV = cvScalar(meanValue_YUV.val[0] + 4 * stdDev_YUV.val[0],meanValue_YUV.val[1] + 4 * stdDev_YUV.val[1],meanValue_YUV.val[2] + 4 * stdDev_YUV.val[2]);

		
}
